/*  Adafruit motor "Shield v1.0" / See: http://www.ladyada.net/make/mshield/index.html
*  Driver v0.1a - Copyright 2011 Arron Chapman
* 
*  Licensed under the Apache License, Version 2.0 (the "License");
*  you may not use this file except in compliance with the License.
*  You may obtain a copy of the License at
*
*      http://www.apache.org/licenses/LICENSE-2.0
*
*  Unless required by applicable law or agreed to in writing, software
*  distributed under the License is distributed on an "AS IS" BASIS,
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*  See the License for the specific language governing permissions and
*  limitations under the License. 
*  
 * 
 * This Adaptation is based on the code provided by Nicolas Ricquemaque
 * the original code can be found at http://code.tinyclr.com/project/323/adafruit-motor-shield-driver/
 * 
*/

using System;
using System.Threading;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoPlus;

namespace DaaseVarmer.helperClasses.HardwareInterface
{
    public class Motorshield : IDisposable
    {
        public enum Motors : byte { M3, M4 }

        private PWM Motor2A, Motor2B; // Enable L293D driver #1a and #1b
        private OutputPort MotorLatch, MotorEnable, MotorClk, MotorData; // 74HCT595 commands
        byte latch_state; // Actual 74HCT595 output state

        /// <summary>
        /// Constructor
        /// </summary>
        public Motorshield()
        {
            Motor2A = new PWM(Pins.GPIO_PIN_D5);
            Motor2A.SetDutyCycle(0);
            Motor2B = new PWM(Pins.GPIO_PIN_D6);
            Motor2B.SetDutyCycle(0);

            MotorLatch = new OutputPort(Pins.GPIO_PIN_D12, true);
            MotorEnable = new OutputPort(Pins.GPIO_PIN_D7, false);
            MotorClk = new OutputPort(Pins.GPIO_PIN_D4, true);
            MotorData = new OutputPort(Pins.GPIO_PIN_D8, true);

            latch_state = 0;
            latch_tx();
        }

        #region IDisposable Members
        public void Dispose()
        {
            latch_state = 0;
            latch_tx();

            Motor2A.Dispose();
            Motor2B.Dispose();

            MotorLatch.Dispose();
            MotorEnable.Dispose();
            MotorClk.Dispose();
            MotorData.Dispose();
        }
        #endregion IDisposable Members


        // Send byte to the 74HCT595 demux
        private void latch_tx()
        {
            MotorLatch.Write(false);
            for (int i = 8; i >= 0; i--)
            {
                MotorClk.Write(false);
                MotorData.Write((latch_state & (1 << i)) > 0);
                MotorClk.Write(true);
            }
            MotorLatch.Write(true);
        }


        /// <summary>
        /// Control a motor; Non-blocking call. Set speed to 0 to stop.
        /// Frequency might need to be adjusted deppending on motor, or to match other PWMs
        /// </summary>
        /// <param name="which">Which motor to drive</param>
        /// <param name="speed">Steed, between 0 and 255 (0 to stop, 255 = max speed)</param>
        /// <param name="direction">True = foward, False = backward</param>
        /// <param name="freq">Frequency in Hz of the PWM controlling the motor</param>
        public void MotorControl(Motors which, byte speed, bool direction, int freq = 100)
        {
            uint period = (uint)(1000000000D / (double)freq);
            switch (which)
            {
                case Motors.M3: // This motor can have its speed controlled through PWM                  
                    if (speed == 0) Motor2B.SetDutyCycle(0);
                    else
                    {
                        latch_state = (byte)((latch_state & 0x5F) | (direction ? 32 : 128));
                        latch_tx();
                        Motor2B.SetPulse(period, (uint) (period*(double) speed/255D));
                    }
                    break;
                case Motors.M4: // This motor can have its speed controlled through PWM                  
                    if (speed == 0) Motor2A.SetDutyCycle(0);
                    else
                    {
                        latch_state = (byte)((latch_state & 0xBE) | (direction ? 1 : 64));
                        latch_tx();
                        Motor2A.SetPulse(period, (uint)(period * (double)speed / 255D));
                    }
                    break;
            }
        }
    }
}